A-star

二维网格A* 算法

记录一下二维网格A*算法实现的知识以及流程

预备知识

A*算法选用原则，选用综合优先级$f(n)$值较小的节点

$$f(n)=g(n)+h(n)$$

其中

$g(n)$表示从上一个父节点移动到指定点的移动代价，比如可以设直走为10，斜走为14，或者根据实际地形确定

$h(n)$表示从指定点到终点的估算成本，它是一个猜测，所以计算时不考虑中间障碍物

$h(n)$也叫做A*算法中的启发函数

常见有曼哈顿距离，对角距离，欧几里得距离

分别对应横竖走，横竖斜走以及任意方向走

设当前位置为$(x,y)$，目标位置为$(x_t,y_t)$，走一步的估算成本为D，则计算公式为

曼哈顿： $h(n) =D*( \vert x_n-x_t \vert + \vert y_n-y_t \vert)$，

对角：$h(n)=D*(\vert x_n-x_t \vert + \vert y_n-y_t \vert + (\sqrt{2}-2)*min(\vert x_n-x_t \vert , \vert y_n-y_t \vert))$

欧几里得：$h(n)=D*\sqrt{({x-x_t})^2+({y-y_t})^2}$

步骤

这里我把步骤列出来，里面的一些细节后文补充

准备一个open_list和close_list，open_list是待遍历的节点，close_list是已遍历的节点

设定一个起点和终点，每一个网格都要记录父节点，$f(n)$，$g(n)$，$h(n)$。

流程：

将起点加入open_list

循环：

	遍历open_list，查找f(n)值最小的节点，作为当前要处理的节点，并把这个节点移到close_list中

	对于当前节点附近的8个方格，剔除掉不可抵达的或者在close_list中的，作如下操作：

		把不在open_list的加入open_list，并把当前节点设为他们的父节点，计算f(n)，g(n)，h(n)

		如果他在open_list，检查这条路径：

			如果路径更好，把节点的父节点设为当前节点，重新计算f(n)，g(n)，h(n)

			如果路径不是更好，则不做处理

	判断终止条件：

		如果终点加入了open_list，则找到路径，从终点不断找父节点回到终点

补充：

1.在刚开始，open_list中只有起点，所以起点也就是要处理的节点

2.计算$g(n)$要累加，表示起点到当前节点$n$的代价和

3.检查路径是为了找出更好的路径，检查的依据是$g(n)$，值越小路径越好。

举个例子就是从中心点A走到B然后再走到C，对比直接从A斜走到C

如果你觉得前者路径更好，则需要把C的父节点设为B，并重新计算它的$f(n)$和$g(n)$值

如果你觉得后者路径更好，则不需要做任何操作，因为C的父节点本来就是中心点A

$$\left[ \begin{matrix} * & * & * \\ * & A & B \\ * & * & C \\ \end{matrix} \right]$$

代码实现

为了验证算法，我用符号简单搭了个二维地图，即省去调试图形化界面的时间，看起来也比较简便

import random
import operator
from math import sqrt


class Node:
    def __init__(self, x, y):
        self.display = 'o'
        self.father_node = None
        self.x = x
        self.y = y
        self.fn = 0
        self.gn = 0
        self.hn = 0


class MyMap:

    def __init__(self):
        # o 表示可走位置 * 表示起点 # 表示终点 @表示障碍物
        self.map_size = (10, 15)  # 高 宽
        self.game_map = [[Node(y, x) for x in range(self.map_size[1])] for y in range(self.map_size[0])]

        # 生成起点和终点 保证起点终点离得有半个地图远
        self.start_point = (0, 0)  # 行 列
        self.end_point = (0, 0)
        self.end_point = self.get_random_point(
            (int(self.map_size[0] / 2), int(self.map_size[1] / 2)),
            self.map_size)
        self.set_object_to_map(self.start_point, "*")
        self.set_object_to_map(self.end_point, "#")

        # 生成障碍物
        self.obstacle = []
        for _ in range(40):
            obstacle_point = self.get_random_point((0, 0), self.map_size)
            if not operator.eq(obstacle_point, self.start_point):
                if not operator.eq(obstacle_point, self.end_point):
                    self.obstacle.append(obstacle_point)
                    self.set_object_to_map(obstacle_point, "@")

    @staticmethod
    def get_random_point(bottom_left, top_right):
        return random.randint(bottom_left[0], top_right[0] - 1), \
               random.randint(bottom_left[1], top_right[1] - 1)

    def set_object_to_map(self, point, obj):
        self.game_map[point[0]][point[1]].display = obj

    def __repr__(self):
        display = ''
        for i in range(self.map_size[0]):
            for j in range(self.map_size[1]):
                display += self.game_map[i][j].display + '  '
            display += '\n'
        return display


def calc_manhattan_distance(start_point, end_point):
    return abs(start_point[0] - end_point[0]) + abs(start_point[1] - end_point[1])


def calc_diagonal_distance(start_point, end_point):
    x_abs = abs(start_point[0] - end_point[0])
    y_abs = abs(start_point[1] - end_point[1])
    return x_abs + y_abs + (sqrt(2) - 2) * min(x_abs, y_abs)


def calc_euclidean_distance(start_point, end_point):
    return sqrt((start_point[0] - end_point[0]) ** 2 + (start_point[1] - end_point[1]) ** 2)


def main():
    my_map = MyMap()
    open_list = []
    close_list = []
    go_straight_cost = 10  # 直走成本
    go_diagonally_cost = 14  # 斜走成本 sqrt(2)
    one_step_cost = 10  # 走一步的成本

    # 将起点加入open_list
    open_list.append(my_map.game_map[my_map.start_point[0]][my_map.start_point[1]])

    while True:
        if len(open_list) <= 0:
            print("没找到路径！\n")
            break

        # 根据fn排序选出当前节点
        open_list = sorted(open_list, key=lambda x: x.fn)
        now_node = open_list[0]

        # 如果选到终点就退出
        if operator.eq((now_node.x, now_node.y), my_map.end_point):
            node = now_node
            # 不断取父节点 原点的父节点是None
            while True:
                node = node.father_node
                if node:
                    if not operator.eq((node.x, node.y), my_map.start_point):  # 避免把原点覆盖
                        my_map.game_map[node.x][node.y].display = '-'
                else:
                    break
            break

        # 把当前节点从open_list移出到close_list
        for i, node in enumerate(open_list):
            if operator.eq((now_node.x, now_node.y), (node.x, node.y)):
                open_list.remove(open_list[i])
                break
        close_list.append(now_node)

        # 提取点位置 方便判断
        open_list_point = list(map(lambda m: (m.x, m.y), open_list))
        close_list_point = list(map(lambda m: (m.x, m.y), close_list))

        # 当前节点附近的八个位置
        for i in range(now_node.x - 1, now_node.x + 2):
            for j in range(now_node.y - 1, now_node.y + 2):
                if not operator.eq((now_node.x, now_node.y), (i, j)):
                    # 越界检查
                    if my_map.map_size[0] > i >= 0 and my_map.map_size[1] > j >= 0:
                        # 如果不是障碍物 也不在close_list中
                        if (i, j) not in my_map.obstacle and (i, j) not in close_list_point:
                            # 如果不在open_list中
                            if (i, j) not in open_list_point:
                                # 加入open_list并设置父节点
                                open_list.append(my_map.game_map[i][j])
                                my_map.game_map[i][j].father_node = now_node
                                # 计算gn
                                if i == now_node.x or j == now_node.y:
                                    my_map.game_map[i][j].gn = go_straight_cost  # 直走
                                else:
                                    my_map.game_map[i][j].gn = go_diagonally_cost  # 斜走
                                last_node = now_node.father_node
                                if last_node:
                                    my_map.game_map[i][j].gn += last_node.gn

                                # 计算hn
                                my_map.game_map[i][j].hn = \
                                    one_step_cost * calc_diagonal_distance((i, j), my_map.end_point)
                                my_map.game_map[i][j].fn = \
                                    my_map.game_map[i][j].gn + \
                                    my_map.game_map[i][j].hn

                            # 如果在open_list中
                            else:
                                # 计算当前路径gn
                                if i == now_node.x or j == now_node.y:
                                    my_map.game_map[i][j].gn = go_straight_cost
                                else:
                                    my_map.game_map[i][j].gn = go_diagonally_cost
                                last_node = now_node.father_node
                                my_map.game_map[i][j].gn += last_node.gn
                                now_path_gn = my_map.game_map[i][j].gn + now_node.gn

                                # 计算新路径gn
                                if i == last_node.x or j == last_node.y:
                                    new_path_gn = go_straight_cost
                                else:
                                    new_path_gn = go_diagonally_cost
                                new_path_gn += last_node.gn

                                # 判断是否更改指向和更新数据
                                if new_path_gn > now_path_gn:
                                    my_map.game_map[i][j].father_node = now_node
                                    my_map.game_map[i][j].gn = now_path_gn
                                    my_map.game_map[i][j].fn = \
                                        my_map.game_map[i][j].gn + \
                                        my_map.game_map[i][j].hn

    print(my_map)


if __name__ == "__main__":
    main()

效果如下：

参考：https://zhuanlan.zhihu.com/p/54510444

启发函数改进

$f(n)=g(n)+h(n)$，

记$k$为结点$n$走到目标结点的步数，则$h(n)$的作用如下：

如果$h(n)=0$，一定能找到最优路径

如果$h(n) \leqslant k$，一定能找到最优路径，$h(n)$越小，拓展节点越多，时间越长

如果$h(n) = k$，一定能找到最优路径，且没有拓展无关节点，时间最短

如果$h(n) \geqslant k$，不一定能找到最优路径，但拓展节点少，时间短

动态加权

开始时$h(n)$较大，到达目标点附近重要，拓展节点少，$h(n)$乘以较大系数

接近终点时$h(n)$较小，找到最优路径重要，拓展节点多，$h(n)$乘以较小系数

可以采用根号动态加权，即

$$h(n) = h(n) * \sqrt{h(n)}$$

内积

计算当前结点到目标节点向量与起点到目标节点向量的内积cross，

$$h(n) = h(n) +0.001*cross$$

内积越大，向量相似度越高，$h(n)$越大